
#include <cassert>
#include <cmath>

#include "Math\Matrix4x4.h"


Matrix4x4::Matrix4x4(const D3DXMATRIX& other)
{
	for(UINT8 i = 0; i < 16; ++i)
        m_matrix[i] = other(i % 4, i / 4);
}


Matrix4x4::Matrix4x4(const Matrix4x4& other)
{
    for(UINT8 i = 0; i < 16; ++i)
        m_matrix[i] = other.m_matrix[i];
}


Matrix4x4& Matrix4x4::operator = (const Matrix4x4& other)
{
    // Self-assignement.
    if(this == &other)
        return *this;

	// Copy.
    for(UINT8 i = 0; i < 16; ++i)
        m_matrix[i] = other.m_matrix[i];

	return *this;
}


Matrix4x4::~Matrix4x4()
{}


Matrix4x4 Matrix4x4::CreateZero()
{
    Matrix4x4 zero;
	ZeroMemory(zero.m_matrix, sizeof(FLOAT) * 16);

    return zero;
}


Matrix4x4 Matrix4x4::CreateIdentity()
{
    Matrix4x4 result = Matrix4x4::CreateZero();

    result.m_matrix[0]  = 1;
    result.m_matrix[5]  = 1;
    result.m_matrix[10] = 1;
    result.m_matrix[15] = 1;

    return result;
}


Matrix4x4 Matrix4x4::CreateTranslation(FLOAT x, FLOAT y, FLOAT z)
{
    Matrix4x4 result = Matrix4x4::CreateIdentity();

    result.m_matrix[3]  = x;
    result.m_matrix[7]  = y;
    result.m_matrix[11] = z;

    return result;
}


Matrix4x4 Matrix4x4::CreateTranslation(const Vector4& vector)
{
    return Matrix4x4::CreateTranslation(vector.GetX(), vector.GetY(), vector.GetZ());
}


Matrix4x4 Matrix4x4::CreateRotation(FLOAT x, FLOAT y, FLOAT z, FLOAT angle)
{
    Matrix4x4 result;

    FLOAT cos = std::cos(angle);
    FLOAT sin = std::sin(angle);

    result.m_matrix[0]  = cos + std::pow(x, 2) * (1 - cos);
    result.m_matrix[1]  = x * y * (1 - cos) - z * sin;
    result.m_matrix[2]  = x * z * (1 - cos) + y * sin;
    result.m_matrix[3]  = 0;
    result.m_matrix[4]  = x * y * (1 - cos) + z * sin;
    result.m_matrix[5]  = cos + std::pow(y, 2) * (1 - cos);
    result.m_matrix[6]  = y * z * (1 - cos) - x * sin;
    result.m_matrix[7]  = 0;
    result.m_matrix[8]  = x * z * (1 - cos) - y * sin;
    result.m_matrix[9]  = y * z * (1 - cos) + x * sin;
    result.m_matrix[10] = cos + std::pow(z, 2) * (1 - cos);
    result.m_matrix[11] = 0;
    result.m_matrix[12] = 0;
    result.m_matrix[13] = 0;
    result.m_matrix[14] = 0;
    result.m_matrix[15] = 1;

    return result;
}


Matrix4x4 Matrix4x4::CreateRotation(const Vector4& axis, FLOAT angle)
{
    return Matrix4x4::CreateRotation(axis.GetX(), axis.GetY(), axis.GetZ(), angle);
}


Matrix4x4 Matrix4x4::CreateScale(FLOAT dx, FLOAT dy, FLOAT dz)
{
    Matrix4x4 result = Matrix4x4::CreateZero();

    result.m_matrix[0]  = dx;
    result.m_matrix[5]  = dy;
    result.m_matrix[10] = dz;
    result.m_matrix[15] = 1;

    return result;
}


Matrix4x4 Matrix4x4::CreateScale(const Vector4& scale)
{
    return Matrix4x4::CreateScale(scale.GetX(), scale.GetY(), scale.GetZ());
}


FLOAT& Matrix4x4::operator () (UINT row, UINT column)
{
	assert(row < 4);
	assert(column < 4);

	return m_matrix[row * 4 + column];
}


const FLOAT& Matrix4x4::operator () (UINT row, UINT column) const
{
	assert(row < 4);
	assert(column < 4);

	return m_matrix[row * 4 + column];
}


Vector4 Matrix4x4::GetRow(UINT index) const
{
	assert(index < 4);

	UINT offset = index * 4;

	return Vector4(m_matrix[offset],
		           m_matrix[offset + 1],
				   m_matrix[offset + 2],
				   m_matrix[offset + 3]);
}


VOID Matrix4x4::SetRow(UINT index, const Vector4& row)
{
	assert(index < 4);

	UINT offset = index * 4;

	m_matrix[offset] = row.GetX();
	m_matrix[offset + 1] = row.GetY();
	m_matrix[offset + 2] = row.GetZ();
	m_matrix[offset + 3] = row.GetW();
}


VOID Matrix4x4::SetRow(UINT index, FLOAT x, FLOAT y, FLOAT z, FLOAT w)
{
	assert(index < 4);

	UINT offset = index * 4;

	m_matrix[offset] = x;
	m_matrix[offset + 1] = y;
	m_matrix[offset + 2] = z;
	m_matrix[offset + 3] = w;
}


Vector4 Matrix4x4::GetColumn(UINT index) const
{
	assert(index < 4);

	return Vector4(m_matrix[index],
				   m_matrix[index + 4],
				   m_matrix[index + 8],
				   m_matrix[index + 12]);
}


VOID Matrix4x4::SetColumn(UINT index, const Vector4& column)
{
	assert(index < 4);

	m_matrix[index] = column.GetX();
	m_matrix[index + 4] = column.GetY();
	m_matrix[index + 8] = column.GetZ();
	m_matrix[index + 12] = column.GetW();
}


VOID Matrix4x4::SetColumn(UINT index, FLOAT x, FLOAT y, FLOAT z, FLOAT w)
{
	assert(index < 4);

	m_matrix[index] = x;
	m_matrix[index + 4] = y;
	m_matrix[index + 8] = z;
	m_matrix[index + 12] = w;
}


Matrix4x4 Matrix4x4::InverseOrthonormal() const
{
    Matrix4x4 result;

    // Because the rotational part is orthonormal, transpose and inversion
    // are the same operation.
    for(UINT8 i = 0; i < 9; ++i)
        result.m_matrix[i % 3 * 4 + i / 3] = m_matrix[i / 3 * 4 + i % 3];

    // Extract translation and apply inverted rotation matrix.
    Vector4 translation(m_matrix[3], m_matrix[7], m_matrix[11]);

    result.m_matrix[3]  = -(Vector4(m_matrix[0], m_matrix[4], m_matrix[8]).Dot(translation));
    result.m_matrix[7]  = -(Vector4(m_matrix[1], m_matrix[5], m_matrix[9]).Dot(translation));
    result.m_matrix[11] = -(Vector4(m_matrix[2], m_matrix[6], m_matrix[10]).Dot(translation));

    // Fill last line.
    result.m_matrix[12] = 0;
    result.m_matrix[13] = 0;
    result.m_matrix[14] = 0;
    result.m_matrix[15] = m_matrix[15];

    return result;
}


// Gram-Schmidt algorithm.
Matrix4x4 Matrix4x4::Orthonormalize() const
{
    Matrix4x4 result;

    Vector4 v2(m_matrix[1], m_matrix[5], m_matrix[9]);
    Vector4 v3(m_matrix[2], m_matrix[6], m_matrix[10]);

    Vector4 u1(m_matrix[0], m_matrix[4], m_matrix[8]);
    Vector4 u2 = v2 - (v2.Dot(u1) / u1.Dot(u1) * u1);
    Vector4 u3 = v3 - (v3.Dot(u1) / u1.Dot(u1) * u1) - (v3.Dot(u2) / u2.Dot(u2) * u2);

    u1 = u1.Normalize();
    u2 = u2.Normalize();
    u3 = u3.Normalize();

    result.m_matrix[0]  = u1.GetX();
    result.m_matrix[1]  = u2.GetX();
    result.m_matrix[2]  = u3.GetX();
    result.m_matrix[3]  = m_matrix[3];
    result.m_matrix[4]  = u1.GetY();
    result.m_matrix[5]  = u2.GetY();
    result.m_matrix[6]  = u3.GetY();
    result.m_matrix[7]  = m_matrix[7];
    result.m_matrix[8]  = u1.GetZ();
    result.m_matrix[9]  = u2.GetZ();
    result.m_matrix[10] = u3.GetZ();
    result.m_matrix[11] = m_matrix[11];
    result.m_matrix[12] = m_matrix[12];
    result.m_matrix[13] = m_matrix[13];
    result.m_matrix[14] = m_matrix[14];
    result.m_matrix[15] = m_matrix[15];

    return result;
}


Matrix4x4 Matrix4x4::Transpose() const
{
    Matrix4x4 result;

    for(UINT8 i = 0; i < 16; ++i)
        result.m_matrix[i % 4 * 4 + i / 4] = m_matrix[i];

    return result;
}


BOOL Matrix4x4::operator == (const Matrix4x4& other) const
{
    if(this == &other)
        return true;

    for(UINT8 i = 0; i < 16; ++i)
        if(m_matrix[i] != other.m_matrix[i])
            return false;

    return true;
}


BOOL Matrix4x4::operator != (const Matrix4x4& other) const
{
    return !(*this == other);
}


Matrix4x4 Matrix4x4::Mul(const Matrix4x4& other) const
{
    Matrix4x4 result;

    for(UINT8 i = 0; i < 4; ++i)
        for(UINT8 j = 0; j < 4; ++j)
            result.m_matrix[i * 4 + j] =
                   m_matrix[i * 4] * other.m_matrix[j] +
                   m_matrix[i * 4 + 1] * other.m_matrix[4 + j] +
                   m_matrix[i * 4 + 2] * other.m_matrix[8 + j] +
                   m_matrix[i * 4 + 3] * other.m_matrix[12 + j];

    return result;
}


Matrix4x4 Matrix4x4::operator * (const Matrix4x4& other) const
{
    return Mul(other);
}


Matrix4x4& Matrix4x4::operator *= (const Matrix4x4& other)
{
    Matrix4x4 result = Mul(other);

    for(UINT8 i = 0; i < 16; ++i)
        m_matrix[i] = result.m_matrix[i];

    return *this;
}


Matrix4x4 Matrix4x4::Mul(FLOAT factor) const
{
	Matrix4x4 result;

	for(UINT i = 0; i < 16; ++i)
		result.m_matrix[i] = m_matrix[i] * factor;

	return result;
}


Matrix4x4 Matrix4x4::operator - () const
{
	return -1 * *this;
}


Vector4 Matrix4x4::Mul(const Vector4& vector) const
{
    Vector4 result;

    result.SetX(m_matrix[0] * vector.GetX() +
                m_matrix[1] * vector.GetY() +
                m_matrix[2] * vector.GetZ() +
                m_matrix[3] * vector.GetW());

    result.SetY(m_matrix[4] * vector.GetX() +
                m_matrix[5] * vector.GetY() +
                m_matrix[6] * vector.GetZ() +
                m_matrix[7] * vector.GetW());

    result.SetZ(m_matrix[8]  * vector.GetX() +
                m_matrix[9]  * vector.GetY() +
                m_matrix[10] * vector.GetZ() +
                m_matrix[11] * vector.GetW());

    result.SetW(m_matrix[12] * vector.GetX() +
                m_matrix[13] * vector.GetY() +
                m_matrix[14] * vector.GetZ() +
                m_matrix[15] * vector.GetW());

    return result;
}


Vector4 Matrix4x4::operator * (const Vector4& vector) const
{
    return Mul(vector);
}


Matrix4x4::operator Quaternion() const
{
    FLOAT trace = m_matrix[0] + m_matrix[5] + m_matrix[10];

    // Directly converted if trace is positive (It's possible to calculate s).
    if(trace > 0)
    {
        FLOAT s = 1 / (2 * std::sqrt(trace));

        return Quaternion((m_matrix[9] - m_matrix[6]) * s,
                          (m_matrix[2] - m_matrix[8]) * s,
                          (m_matrix[4] - m_matrix[1]) * s,
                          s / 4);
    }
    // Otherwise find highest number in diagonal.
    else
    {
		// Don't undestand why std::max isn't working here.
        FLOAT max = m_matrix[0] > m_matrix[5] ? m_matrix[0] : m_matrix[5];
		max = max > m_matrix[10] ? max : m_matrix[10];

        if(max == m_matrix[0])
        {
            FLOAT s = std::sqrt(1 + m_matrix[0] - m_matrix[5] - m_matrix[10]) * 2;

            return Quaternion(s / 2,
                             (m_matrix[1] - m_matrix[4]) / s,
                             (m_matrix[2] - m_matrix[8]) / 2,
                             (m_matrix[6] - m_matrix[9]) / 2);
        }
        else if(max == m_matrix[5])
        {
            FLOAT s = std::sqrt(1.0f - m_matrix[0] + m_matrix[5] - m_matrix[10]) * 2;

            return Quaternion((m_matrix[1] - m_matrix[4]) / s,
                              s / 2,
                              (m_matrix[6] - m_matrix[9]) / 2,
                              (m_matrix[2] - m_matrix[8]) / 2);
        }
        else
        {
            FLOAT s = std::sqrt(1.0f - m_matrix[0] - m_matrix[5] + m_matrix[10]) * 2;

            return Quaternion((m_matrix[2] - m_matrix[8]) / s,
                              (m_matrix[6] - m_matrix[9]) / 2,
                              s / 2,
                              (m_matrix[1] - m_matrix[4]) / 2);
        }
    }
}


Matrix4x4::operator D3DXMATRIX() const
{
	return D3DXMATRIX(this->Transpose().m_matrix);
}


Matrix4x4::Matrix4x4()
{}


Matrix4x4 operator * (const Matrix4x4& matrix, FLOAT factor)
{
	return matrix.Mul(factor);
}


Matrix4x4 operator * (FLOAT factor, const Matrix4x4& matrix)
{
	return matrix.Mul(factor);
}